//     This program is free software: you can redistribute it and/or modify
//     it under the terms of the GNU General Public License as published by
//     the Free Software Foundation, either version 3 of the License, or
//     (at your option) any later version.
// 
//     This program is distributed in the hope that it will be useful,
//     but WITHOUT ANY WARRANTY; without even the implied warranty of
//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//     GNU General Public License for more details.
// 
//     You should have received a copy of the GNU General Public License
//     along with this program.  If not, see <http://www.gnu.org/licenses/>.

/*****************************************************************************
//  Includes
*****************************************************************************/

// #include "comports.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
       
#include <string.h>
#include <inttypes.h>

#define SNUM_LEFT 4
#define SNUM_RIGHT 5

#define ACCEL 4
#define INC 2

int tty_open(char name[128], speed_t bspeed)
{
	struct termios tios;
	int fd, res;

    fd = open(name, O_RDWR | O_NONBLOCK);

    if (fd < 0)
    {
        fprintf(stderr, "Unable to open port %s\n\r", name);
        return (fd);
    }

    tios.c_cflag = CS8 | CREAD | CLOCAL;
    tios.c_iflag = 0;
    tios.c_oflag = 0;
    tios.c_lflag = 0;

    tios.c_cc[VTIME] = 0;//vtime;                                  // Port timeout: 1/10ths of a second (vtime = 1)
    tios.c_cc[VMIN] = 0;//vmin;                                    // Minimum number of chars to read (vmin = 0)

    cfsetispeed(&tios, bspeed);
    cfsetospeed(&tios, bspeed);

    res = tcsetattr(fd, TCSANOW, &tios);
    if (res)
    {
        fprintf(stderr, "termios: tcsetattr() failed\n\r");
        return (-1);
    }

    return (fd);
}

set_servo(int fd, uint8_t servo_num, uint8_t accel, uint8_t speed)
{
    uint8_t b[5];
    int res;
        
    b[0] = 0x80;
    b[1] = 0x01;
    b[2] = 0x01;
    b[3] = servo_num;
    b[4] = accel;
    
    res = write(fd, b, 5);
    if(res!=5) printf("res = %d", res);
    
    b[2] = 0x02;
    b[3] = servo_num;
    b[4] = speed;
    
    res = write(fd, b, 5);
    if(res!=5) printf("res = %d", res);
}

int main(int argc, char ** argv)
{
    int i,tmp[5];
    int fd, res, input;
    uint8_t run_flag = 1;
    uint8_t speed_left = 64, speed_right = 64;

    system("stty raw -echo");
    
    fd = tty_open("/dev/ttyS1", B9600);
    
    if (fd > 0)
    {
        while(run_flag)
        {
            input = getchar();
            printf("input = %x\n\r", input);
            switch(input)
            {
                case 0x03:
                    run_flag = 0;
                    break;
                
                case 0x1b:
                    if(getchar()==0x5b)
                    {
                        switch(getchar())
                        {
                            case 0x41:
                                printf("forward\n\r");
                                if((speed_left>0) && (speed_left<255)) speed_left+=INC;
                                if((speed_right>0) && (speed_right<255)) speed_right-=INC;
                                set_servo(fd, SNUM_LEFT, ACCEL, speed_left);
                                set_servo(fd, SNUM_RIGHT, ACCEL, speed_right);
                                break;
                                
                            case 0x42:
                                printf("back\n\r");
                                if((speed_left>0) && (speed_left<255)) speed_left-=INC;
                                if((speed_right>0) && (speed_right<255)) speed_right+=INC;
                                set_servo(fd, SNUM_LEFT, ACCEL, speed_left);
                                set_servo(fd, SNUM_RIGHT, ACCEL, speed_right);
                                break;
                            
                            case 0x43:
                                printf("right\n\r");
                                if((speed_left>0) && (speed_left<255)) speed_left+=INC;
                                if((speed_right>0) && (speed_right<255)) speed_right+=INC;
                                set_servo(fd, SNUM_LEFT, ACCEL, speed_left);
                                set_servo(fd, SNUM_RIGHT, ACCEL, speed_right);
                                break;
                                
                            case 0x44:
                                printf("left\n\r");
                                if((speed_left>0) && (speed_left<255)) speed_left-=INC;
                                if((speed_right>0) && (speed_right<255)) speed_right-=INC;
                                set_servo(fd, SNUM_LEFT, ACCEL, speed_left);
                                set_servo(fd, SNUM_RIGHT, ACCEL, speed_right);
                                break;
                        }
                    }
                    break;
                
                case 0x20:
                    speed_left = 64;
                    speed_right = 64;
                    set_servo(fd, SNUM_LEFT, ACCEL, speed_left);
                    set_servo(fd, SNUM_RIGHT, ACCEL, speed_right);
                    printf("stop\n\r");
                    break;
            }
            
        }
        system("stty cooked echo");
        close(fd);
        return 0;
    } else
    {
        printf("unable to open file\n");
        return -1;
    }
}

